#pragma once

#include "Eigen/Dense"
#include <vector>
#include <cmath>
using namespace std;
namespace Robot{
    typedef Eigen::Matrix<float, 4, 4> Matrix44f;
    typedef Eigen::Matrix<float,1,4> Matrix14f;

    class Link{
    public:
        Link(float a,float d,float alpha,float theta) {
            mLength = a;
            mOffset = d;
            mTheta = theta;
            mAlpha = alpha;
        };
        float Theta(){
            return mTheta;
        }
        Matrix44f Transform() {
            Matrix44f ret;
            ret<<   cos(mTheta),-sin(mTheta)*cos(mAlpha),sin(mTheta)*sin(mAlpha),mLength*cos(mTheta),
                    sin(mTheta),cos(mTheta)*cos(mAlpha),-sin(mAlpha)*cos(mTheta),mLength*sin(mTheta),
                    0,sin(mAlpha),cos(mAlpha),mOffset,
                    0,0,0,1;
            return ret;
        }
        Matrix44f Transform(float theta) {
            mTheta = theta;
            Matrix44f ret;
            ret<<   cos(theta),-sin(theta)*cos(mAlpha),sin(theta)*sin(mAlpha),mLength*cos(theta),
                    sin(theta),cos(theta)*cos(mAlpha),-sin(mAlpha)*cos(theta),mLength*sin(theta),
                    0,sin(mAlpha),cos(mAlpha),mOffset,
                    0,0,0,1;
            return ret;
        }
    private:
        float mLength;   // D
        float mOffset;   // A
        float mTheta;   //
        float mAlpha;
    };
    class Robot{
    public:
        Robot(int axis){
            mLinkCount = axis;
        };
        void AppendLink(Link l){
            mLinks.push_back(l);
        };
        Matrix44f Pose(){
            Matrix44f ret ;
            ret<< 1,0,0,0,
                    0,1,0,0,
                    0,0,1,0,
                    0,0,0,1;
            for(Link v:mLinks) {
                ret = ret*v.Transform();
            }
            return ret;
        };
    private:
        vector<Link> mLinks;
        int mLinkCount;
    };
}


